#include "vehicle_ll_socp_mpc.h"
#include "poly2d.h"
#include <cmath>
namespace auto_ros
{
namespace control
{

VehicleLLSocpMpc::VehicleLLSocpMpc(const std::string dyn_mpc_kf_config_path, const std::string vehicle_lmpc_yaml_name, double rate)
	: VehicleCvxMpcBase(dyn_mpc_kf_config_path, vehicle_lmpc_yaml_name, rate)
{
	init_mpc();
	dynpar_map_ = {{"speed", 1.0}};
	//init ref_state_solver after add cost and constraints
	ref_state_slover_ptr_->add_slack_states({0, 4});
	ref_state_slover_ptr_->qp_.addConstraint(box(-10. / 180.0 * M_PI,
												 (ref_state_slover_ptr_->x_(1) + par(vehicle_dyn_ptr_->parm_["lf"]) * ref_state_slover_ptr_->x_(2)) / dynpar(dynpar_map_["speed"]) - ref_state_slover_ptr_->u_(0),
												 10. / 180.0 * M_PI));
	ref_state_slover_ptr_->qp_.addConstraint(box(-10. / 180.0 * M_PI,
												 (ref_state_slover_ptr_->x_(1) - par(vehicle_dyn_ptr_->parm_["lr"]) * ref_state_slover_ptr_->x_(2)) / dynpar(dynpar_map_["speed"]),
												 10. / 180.0 * M_PI));
	ref_state_slover_ptr_->init_solver_ptr();

} //
void VehicleLLSocpMpc::init_mpc()
{
	//init mpc_solver after add cost and constraints
	unsigned int mpc_windows = mpc_yaml_["mpc_windows"].as<int>();

	cvx_mpc_ptr_ = std::make_shared<SocLpvMPC>(mpc_windows, dis_dyn_ptr_);
	cost_var_ = cvx_mpc_ptr_->qp_.addVariable("cost", 1);
	slip_angle_var_ = cvx_mpc_ptr_->qp_.addVariable("silp_angle", mpc_windows, 2);
	////add tire_friction constraints

	tire_circle_slack_ = cvx_mpc_ptr_->qp_.addVariable("tire_slack", mpc_windows, 2);
	for (unsigned int i = 0; i++; i < mpc_windows)
	{
		//front wheel
		//cvx_mpc_ptr_->qp_.addConstraint();
		//rear_wheel
	}
	cvx_mpc_ptr_->init_cvx_solver();
}
bool VehicleLLSocpMpc::step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
							const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
							double speed, const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, double tire_f_alpha_w,
							double tire_r_alpha_w)
{
}
bool VehicleLLSocpMpc::step(const Eigen::VectorXd &x0,
							const Eigen::VectorXd &u_fb,
							const PlanningRefPoint &ref_point,
							double speed)
{
}
void VehicleLLSocpMpc::update_friction_circle(double speed)
{
	speed = eliminate_range<double>(speed, -1.0, 0.0, 1.0);
	//cvx_mpc_ptr_->qp_.addConstraint();
}
bool VehicleLLSocpMpc::reset_kf()
{
	Eigen::MatrixXd state_cov_esti =
		Eigen::MatrixXd::Zero(kf_lateral_ptr_->con_ptr_->state_dim(),
							  kf_lateral_ptr_->con_ptr_->state_dim());
	Eigen::VectorXd state_esti =
		Eigen::VectorXd::Zero(kf_lateral_ptr_->con_ptr_->state_dim());
	return kf_lateral_ptr_->reset(state_esti, state_cov_esti);
}
bool VehicleLLSocpMpc::reset_kf(const Eigen::VectorXd &state_esti)
{
	Eigen::MatrixXd state_cov_esti =
		Eigen::MatrixXd::Zero(kf_lateral_ptr_->con_ptr_->state_dim(), kf_lateral_ptr_->con_ptr_->state_dim());
	return kf_lateral_ptr_->reset(state_esti, state_cov_esti);
}
} // namespace control
} // namespace auto_ros
